
#include <iostream>
#include <fstream>
#include <thread>

#include <opencv2/opencv.hpp>

#include "TrackerKCFdsp.hpp"

#include <random>

#include "utils.hpp"

using namespace std;
using namespace cv;

void test_dsp_main(int x, int y, int w, int h, int patch_size, const char *in_path, const char *out_path){
    Mat frame;
    Rect curr_res;
    Rect prev_res;
    int key;
    // Rect ROI(520, 470, 128, 128);
    Rect ROI(x, y, w, h);


    int32_t detect_dsp_usec;
    int32_t update_dsp_usec;
    char img_file_name[1024] = {0};
    char out_img_file_name[1024] = {0};
    int frame_cnt = 0;
    sprintf(img_file_name, "%s/%04d.png", in_path, frame_cnt);
    frame = imread(img_file_name, IMREAD_GRAYSCALE);
    Size S = Size((int)frame.cols, (int)frame.rows);
    cout << "image size:" << S << endl;
    sprintf(out_img_file_name, "%s/log.txt", out_path);
    ofstream log(out_img_file_name, ios::out|ios::trunc);
    random_device rd;  // will be used to obtain a seed for the random number engine
    mt19937 gen(rd()); // standard mersenne_twister_engine seeded with rd()
    uniform_int_distribution<> distrib(-48, 48);

    TrackerKCFdsp::Ptr tracker = TrackerKCFdsp::Ptr(
        new TrackerKCFdsp(S.width, S.height, patch_size, false)
    );
    curr_res = ROI;
    tracker->update(curr_res, frame, 1.f, &update_dsp_usec);
    prev_res = curr_res;
    int offset_x = 0;//distrib(gen);
    int offset_y = 0;//distrib(gen);
    prev_res.x += offset_x;
    prev_res.y += offset_y;
    cout << "offset: " << offset_x << ", " << offset_y << endl;
    log << "detect, update, 128" << endl;
    while (frame_cnt < 200) {
        sprintf(img_file_name, "%s/%04d.png", in_path, frame_cnt);
        frame = imread(img_file_name);
        if (frame.empty()) {
            break;
        }
        Mat gray;
        cvtColor(frame, gray, COLOR_BGR2GRAY);
        TicToc tic;
        float curr_peak_value = 0.f;
        curr_res = tracker->detect(prev_res, gray, curr_peak_value, &detect_dsp_usec);
        uint64_t detect_cost = tic.toc();
        tic.tic();
        // if (curr_peak_value < 0.4f) break;
        // tracker->update(curr_res, gray, 0.01f, &update_dsp_usec);
        tracker->update(curr_res, 0.01f, &update_dsp_usec);
        uint64_t update_cost = tic.toc();
        cout << frame_cnt << " peak_value: " << curr_peak_value << ", detect: " << detect_cost << "us, update: " << update_cost << "us"  << endl;
        rectangle(
            frame,
            Point(prev_res.x, prev_res.y),
            Point(prev_res.x + prev_res.width, prev_res.y + prev_res.height),
            Scalar(255, 0, 0), 2, 8
        );
        prev_res = curr_res;
        offset_x = distrib(gen);
        offset_y = distrib(gen);
        prev_res.x += offset_x;
        prev_res.y += offset_y;
        cout << "offset: " << offset_x << ", " << offset_y << endl;
        rectangle(
            frame,
            Point(curr_res.x, curr_res.y),
            Point(curr_res.x + curr_res.width, curr_res.y + curr_res.height),
            Scalar(0, 255, 0), 2, 8
        );
        #if 1
        sprintf(out_img_file_name, "%s/%04d.png", out_path, frame_cnt);
        // cout << out_img_file_name << endl;
        imwrite(out_img_file_name, frame);
        #endif
        log << detect_cost << ", " << update_cost << ", " << detect_dsp_usec << ", " << update_dsp_usec << endl;
        frame_cnt++;
        // std::this_thread::sleep_for(std::chrono::milliseconds(20)); 
    }
    if (false)
    {
        int64_t* buf = (int64_t*)malloc(128*128*8);
        tracker->get_debug(buf, 128*128*sizeof(int64_t));
        char dbg_file_name[1024];
        sprintf(dbg_file_name, "%s/dbg_data.txt", out_path);
        ofstream dbg_out(dbg_file_name, ios::out|ios::trunc);
        // int16_t *p = (int16_t *)buf;
        // int64_t *p = (int64_t *)buf;
        int32_t *p = (int32_t *)buf;
        for (int i = 0; i < 128; i++) {
            for (int j = 0; j < 128; j++) {
                // dbg_out << static_cast<float>(p[i*128+j])/(1<<15) << ", "; 
                // float re = static_cast<float>(p[i*128+j]&0x0ffffffff)/(1<<16);
                // float im = static_cast<float>(p[i*128+j]>>32)/(1<<16);
                float re = static_cast<float>(p[i*256+2*j])/(1<<8);
                float im = static_cast<float>(p[i*256+2*j+1])/(1<<8);
                dbg_out << re <<","<<im <<","; 
            }
            dbg_out << endl;
        }
        free(buf);
    }
}
